#include "Kinematics.h"

void Kinematics::set_motor_param(uint8_t id, float per_pulse_distance)
{
    motor_param_[id].per_pulse_distance = per_pulse_distance;
}

void Kinematics::set_wheel_distance(float wheel_distance)
{
    wheel_distance_ = wheel_distance;
}

void Kinematics::kinematic_inverse(float linear_speed, float angle_speed, float &out_left_speed, float &out_right_speed)
{
    out_left_speed = linear_speed - (angle_speed * wheel_distance_) / 2.0;
    out_right_speed = linear_speed + (angle_speed * wheel_distance_) / 2.0;
}

void Kinematics::kinematic_forward(float left_speed, float right_speed, float &out_linear_speed, float &out_angle_speed)
{
    // 两轮转速之和除以2
    out_linear_speed = (right_speed + left_speed) / 2.0;
    // 两轮转速之差除以轮距
    out_angle_speed = (right_speed - left_speed) / wheel_distance_;
}

void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick, int32_t right_tick)
{
    // 计算出上次更新以来经过的时间dt
    uint32_t dt = current_time - last_update_time_;
    last_update_time_ = current_time;
    // 计算电机1和电机2的编码器读数变化量
    int32_t dtick1 = left_tick - motor_param_[0].last_encoder_tick;
    int32_t dtick2 = left_tick - motor_param_[1].last_encoder_tick;
    motor_param_[0].last_encoder_tick = left_tick;
    motor_param_[1].last_encoder_tick = right_tick;
    // 轮子速度计算
    motor_param_[0].motor_speed = float(dtick1 * motor_param_[0].per_pulse_distance) / dt * 1000;
    motor_param_[1].motor_speed = float(dtick1 * motor_param_[1].per_pulse_distance) / dt * 1000;
}

int16_t Kinematics::get_motor_speed(uint8_t id)
{
    return motor_param_[id].motor_speed;
}